//
// Created by 刘洋 on 2024/2/13.
//
#include "lowpass_filter.h"

float LowPassFilterInit(LowPassFilter *lpf, float time_constant)
{
    lpf->Tf = time_constant;
    lpf->y_prev = 0.0f;
    lpf->timestamp_prev = micros();
}
float LowPassFilterRun(LowPassFilter *lpf, float x)
{
    unsigned long timestamp = micros();
    float dt = ((float) timestamp - (float) lpf->timestamp_prev) * 1e-6f;

    if (dt < 0.0f) dt = 1e-3f;
    else if (dt > 0.3f)
    {
        lpf->y_prev = x;
        lpf->timestamp_prev = timestamp;
        return x;
    }

    float alpha = lpf->Tf / (lpf->Tf + dt);
    float y = alpha * lpf->y_prev + (1.0f - alpha) * x;
    lpf->y_prev = y;
    lpf->timestamp_prev = timestamp;
    return y;
}
